#include <cmath>
#include <cstdio>

#include <limits>

#include "DlgCalibVec.h"

DlgCalibVec::DlgCalibVec(QWidget *parent)
: QDialog(parent)
, _dev(NULL)
{
	setupUi(this);
}

void DlgCalibVec::setDevice(z800_device *dev)
{
	_dev = dev;
}

void DlgCalibVec::setRawData(const QVector3D &gyr, const QVector3D &acc, const QVector3D &mag)
{
	_gyr = gyr;
	_acc = acc;
	_mag = mag;
}

void DlgCalibVec::on_startPushButton_pressed()
{
	_magMeasurement.clear();
	_accMeasurement.clear();
	startPushButton->setEnabled(false);
	takeMeasurementPushButton->setEnabled(true);
}

void DlgCalibVec::on_takeMeasurementPushButton_pressed()
{
	_accMeasurement.push_back(_acc);
	_magMeasurement.push_back(_mag);
	if(_accMeasurement.size() > 3)
	{
		calculatePushButton->setEnabled(true);
	}
}

void DlgCalibVec::on_calculatePushButton_pressed()
{
	startPushButton->setEnabled(true);
	takeMeasurementPushButton->setEnabled(false);
	calculatePushButton->setEnabled(false);

	double inf = std::numeric_limits<double>::infinity();

	QVector3D accMin(+inf, +inf, +inf);
	QVector3D accMax(-inf, -inf, -inf);
	QVector3D magMin(+inf, +inf, +inf);
	QVector3D magMax(-inf, -inf, -inf);

	foreach(const QVector3D &v, _accMeasurement)
	{
		if(v.x() < accMin.x()) accMin.setX(v.x());
		if(v.x() > accMax.x()) accMax.setX(v.x());
		if(v.y() < accMin.y()) accMin.setY(v.y());
		if(v.y() > accMax.y()) accMax.setY(v.y());
		if(v.z() < accMin.z()) accMin.setZ(v.z());
		if(v.z() > accMax.z()) accMax.setZ(v.z());
	}

	foreach(const QVector3D &v, _magMeasurement)
	{
		if(v.x() < magMin.x()) magMin.setX(v.x());
		if(v.x() > magMax.x()) magMax.setX(v.x());
		if(v.y() < magMin.y()) magMin.setY(v.y());
		if(v.y() > magMax.y()) magMax.setY(v.y());
		if(v.z() < magMin.z()) magMin.setZ(v.z());
		if(v.z() > magMax.z()) magMax.setZ(v.z());
	}

	QVector3D accOffset;
	QVector3D magOffset;

	accOffset.setX( - (accMin.x() + accMax.x()) / 2);
	accOffset.setY( - (accMin.y() + accMax.y()) / 2);
	accOffset.setZ( - (accMin.z() + accMax.z()) / 2);

	magOffset.setX( - (magMin.x() + magMax.x()) / 2);
	magOffset.setY( - (magMin.y() + magMax.y()) / 2);
	magOffset.setZ( - (magMin.z() + magMax.z()) / 2);

	QVector3D accGain;
	QVector3D magGain;

	accGain.setX(1 / (- accMin.x() + accMax.x()) * 2);
	accGain.setY(1 / (- accMin.y() + accMax.y()) * 2);
	accGain.setZ(1 / (- accMin.z() + accMax.z()) * 2);
	magGain.setX(1 / (- magMin.x() + magMax.x()) * 2);
	magGain.setY(1 / (- magMin.y() + magMax.y()) * 2);
	magGain.setZ(1 / (- magMin.z() + magMax.z()) * 2);

	double accGainGoodness = 0.0;

	double avgLength = 0.0;
	foreach(const QVector3D &v, _accMeasurement)
	{
		QVector3D accTest = (v + accOffset) * accGain;
		avgLength += accTest.length();
	}
	avgLength /= _accMeasurement.size();

	foreach(const QVector3D &v, _accMeasurement)
	{
		QVector3D accTest = (v + accOffset) * accGain;
		accGainGoodness += fabs(accTest.length() - avgLength);
	}

	double magGainGoodness = 0.0;

	avgLength = 0.0;
	foreach(const QVector3D &v, _magMeasurement)
	{
		QVector3D magTest = (v + magOffset) * magGain;
		avgLength += magTest.length();
	}
	avgLength /= _magMeasurement.size();

	foreach(const QVector3D &v, _magMeasurement)
	{
		QVector3D magTest = (v + magOffset) * magGain;
		magGainGoodness += fabs(magTest.length() - avgLength);
	}

	QVector3D bestAccGain = accGain;
	QVector3D bestMagGain = magGain;

	for(int xgi = -500; xgi < 500; ++xgi)
	{
		double xg = double(xgi) / 5000;
		for(int ygi = -500; ygi < 500; ++ygi)
		{
			double yg = double(ygi) / 5000;

			double newAccGainGoodness = 0.0;

			QVector3D testAccGain = accGain;
			testAccGain.setX(testAccGain.x() + xg);
			testAccGain.setY(testAccGain.y() + yg);

			double avgLength = 0.0;
			foreach(const QVector3D &v, _accMeasurement)
			{
				QVector3D accTest = (v + accOffset) * testAccGain;
				avgLength += accTest.length();
			}
			avgLength /= _accMeasurement.size();


			foreach(const QVector3D &v, _accMeasurement)
			{
				QVector3D accTest = (v + accOffset) * testAccGain;
				newAccGainGoodness += fabs(accTest.length() - avgLength);
			}

			double newMagGainGoodness = 0.0;

			QVector3D testMagGain = magGain;
			testMagGain.setX(testMagGain.x() + xg);
			testMagGain.setY(testMagGain.y() + yg);

			avgLength = 0.0;
			foreach(const QVector3D &v, _magMeasurement)
			{
				QVector3D magTest = (v + magOffset) * testMagGain;
				avgLength += magTest.length();
			}
			avgLength /= _accMeasurement.size();

			foreach(const QVector3D &v, _magMeasurement)
			{
				QVector3D magTest = (v + magOffset) * testMagGain;
				newMagGainGoodness += fabs(magTest.length() - avgLength);
			}

			if(newAccGainGoodness < accGainGoodness)
			{
				accGainGoodness = newAccGainGoodness;
				bestAccGain = testAccGain;
			}

			if(newMagGainGoodness < magGainGoodness)
			{
				magGainGoodness = newMagGainGoodness;
				bestMagGain = testMagGain;
			}
		}
	}

	accLineEdit->setText(QString::number(accGainGoodness));
	magLineEdit->setText(QString::number(magGainGoodness));

	struct z800_calibration_data calibrationData;

	z800_get_calibration_data(_dev, &calibrationData);

	calibrationData.acc_offset[0] = accOffset.x();
	calibrationData.acc_offset[1] = accOffset.y();
	calibrationData.acc_offset[2] = accOffset.z();
	calibrationData.acc_gain[0] = bestAccGain.x();
	calibrationData.acc_gain[1] = bestAccGain.y();
	calibrationData.acc_gain[2] = bestAccGain.z();
	calibrationData.mag_offset[0] = magOffset.x();
	calibrationData.mag_offset[1] = magOffset.y();
	calibrationData.mag_offset[2] = magOffset.z();
	calibrationData.mag_gain[0] = bestAccGain.x();
	calibrationData.mag_gain[1] = bestMagGain.y();
	calibrationData.mag_gain[2] = bestMagGain.z();

	z800_set_calibration_data(_dev, &calibrationData);
}
